% normalized outputs
% function main_normalized(model)
addpath('./Func/')
addpath('./HumanData')
addpath('./PlotFunc')
% name list 9 person
name  = {'Charles','Fred','Martin','Ram','Selina',... % 1st experiment
    'lily','po','ryan','vic'};  % 2nd experiment

% CONSTANT
% the number of subjects from the 1st experiment
exp1st = 5;
% the index of subjects use the right leg as swing leg during the step
rIndex = [6,7,9];
% experimental sampling frequency
fs = 480;
% % model = 'NAO';
model = 'HM';
% model  = 'AMBER';
time_rec = zeros(9,1);
for index = 1:length(name) % 5
    
    % get human data, with breaking pts for each step
    humanData = getData(name{1,index});
    
    %     sum_weight  = sum_weight+humanData.weight; % for average weight
    % breaking pts for each step
    StartPt = humanData.sns_brk(2);
    EndPt = humanData.sns_brk(3);
    
    % position
    if index <= exp1st
        [xpos,ypos,zpos] = getPosCartesian(humanData); % subjects from experiment 1st
    else
        [xpos,ypos,zpos] = getPosCartesianNew(humanData); % subjects from experiment 2st
    end
    
    %%%%%%%%%%%  time  %%%%%%%%%%%%%%
    % scale time
    % from [c:d]
    ns_time_pre = (0:length(StartPt:EndPt)-1)'/fs;
    ns_ret = ns_time_pre;
    xpara_time = 1/ns_ret(end); % simply sacled time to 1
    ns_time = ns_ret*xpara_time;
    %%%% record time %%%%%%%%
    time_rec(index) = ns_ret(end);
    
    
    
    %%%%%%%%%%%% calculate human joint angles %%%%%%%%%%%%%%%%%
    if ismember(index,rIndex)
        angle = Knee2D_angle(xpos,ypos,zpos,humanData.sns_brk,2);
    else
        angle = Knee2D_angle(xpos,ypos,zpos,humanData.sns_brk,1);
    end
    
    %%%%%%%%%%% use joint angles to calculate the outputs %%%%%%%%%%
%     q = [angle.sa;angle.sknee;angle.theta3;angle.theta4;angle.nsknee];
    % for non-torso
    q = [angle.sa;angle.sknee;angle.theta3-angle.theta4;angle.nsknee];
    %%%%%%%%%%% NEED TO CHANGE HERE WHEN ADDING OUTPUTS %%%%%%%%%%%%%%%%%%
    outputs = cal_robot_para_18(q,model);
 
    
    
end


